#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <math.h>
#include <iostream>
#include <vector>

using namespace std;
// #include <turtlesim/Spawn.h>

int main(int argc, char** argv){
    ros::init(argc, argv, "my_tf_listener");

    ros::NodeHandle node;

    // 给跟随机器人发布速度命令（用于测试）
    // ros::Publisher tb3_1_vel = node.advertise<geometry_msgs::Twist>("tb3_1/cmd_vel", 10);
    // ros::Publisher tb3_2_vel = node.advertise<geometry_msgs::Twist>("tb3_2/cmd_vel", 10);

    // 给跟随机器人发布控制命令
    ros::Publisher tb3_1_pos = node.advertise<geometry_msgs::Point>("tb3_1_pos", 10);
    ros::Publisher tb3_2_pos = node.advertise<geometry_msgs::Point>("tb3_2_pos", 10);

    tf::TransformListener listener_1, listener_2;   //创建监听对象

    ros::Rate rate(10.0);
    listener_1.waitForTransform("/tb3_1/base_link", "/carrot_r", ros::Time(0), ros::Duration(8.0));
    listener_2.waitForTransform("/tb3_2/base_link", "/carrot_l", ros::Time(0), ros::Duration(8.0));
    while (node.ok()){
        tf::StampedTransform transform_1;
        tf::StampedTransform transform_2;
        try{
            listener_1.waitForTransform("/tb3_1/base_link", "/carrot_r", ros::Time(0), ros::Duration(8.0));
            listener_2.waitForTransform("/tb3_2/base_link", "/carrot_l", ros::Time(0), ros::Duration(8.0));
            // 不可以把ros::Time(0)改成ros::time::now()，因为监听做不到实时，会有几毫秒的延迟，除非需要结合waitForTransform()使用
            listener_1.lookupTransform("/tb3_1/base_link", "/carrot_r", ros::Time(0), transform_1);   
            listener_2.lookupTransform("/tb3_2/base_link", "/carrot_l", ros::Time(0), transform_2);
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        // 发布tb3_1当前位置
        geometry_msgs::Point tb3_1_pos_msg;   //位置消息
        float x1, y1;
        x1 = transform_1.getOrigin().x();
        y1 = transform_1.getOrigin().y();
        tb3_1_pos_msg.x = (int)((x1 * 100) + 0.5) / 100.0;
        tb3_1_pos_msg.y = (int)((y1 * 100) + 0.5) / 100.0;
        tb3_1_pos.publish(tb3_1_pos_msg);


        geometry_msgs::Point tb3_2_pos_msg;   //位置消息
        float x2, y2;
        x2 = transform_2.getOrigin().x();
        y2 = transform_2.getOrigin().y();
        tb3_2_pos_msg.x = (int)((x2 * 100) + 0.5) / 100.0;
        tb3_2_pos_msg.y = (int)((y2 * 100) + 0.5) / 100.0;
        tb3_2_pos.publish(tb3_2_pos_msg);

        ROS_INFO("x_r=%f, y_r=%f, x_l=%f, y_l=%f",tb3_1_pos_msg.x ,tb3_1_pos_msg.y ,tb3_2_pos_msg.x , tb3_2_pos_msg.y);  // 打印相对坐标信息

/*
        float x_1 = fabs(transform_1.getOrigin().x());
        float y_1 = fabs(transform_1.getOrigin().y());
        float lenth_1 = sqrt(pow(x_1, 2) +pow(x_1, 2));

        float x_2 = fabs(transform_2.getOrigin().x());
        float y_2 = fabs(transform_2.getOrigin().y());
        float lenth_2 = sqrt(pow(x_2, 2) +pow(y_2, 2));        

        // 设定跟随机器人速度
        // tb3_1(right)
        geometry_msgs::Twist vel_msg;   //速度消息
        if(lenth_1 <= 0.01)
        {
            vel_msg.angular.z = 0;
            vel_msg.linear.x = 0;
            tb3_1_vel.publish(vel_msg);
        }
        else
        {
            vel_msg.angular.z = 0.5 * atan2(transform_1.getOrigin().y(),
                                            transform_1.getOrigin().x());
            vel_msg.linear.x = 0.5 * sqrt(pow(transform_1.getOrigin().x(), 2) +
                                        pow(transform_1.getOrigin().y(), 2));
            tb3_1_vel.publish(vel_msg);
        }

        // tb3_2(left)
        if(lenth_2 <= 0.01)
        {
            vel_msg.angular.z = 0;
            vel_msg.linear.x = 0;
            tb3_2_vel.publish(vel_msg);
        }
        else
        {
            vel_msg.angular.z = 0.5 * atan2(transform_2.getOrigin().y(),
                                            transform_2.getOrigin().x());
            vel_msg.linear.x = 0.5 * sqrt(pow(transform_2.getOrigin().x(), 2) +
                                        pow(transform_2.getOrigin().y(), 2));
            tb3_2_vel.publish(vel_msg);
        }
*/

        rate.sleep();
    }
    return 0;
};